#include <iostream>
#include <string>
#include <unordered_map>
#include <limits>

#include "Arm.h"
#include "serialib.h"
#include "packets/XenoSlavePacket.h"
#include "LiftStretch.h"
#include "ShiftSuck.h"

void clearInputBuffer() {
    std::cin.clear();
    std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}

int main() {
    // Define valid commands
    std::unordered_map<std::string, float *> commandMap;
    XenoSlavePacket packet{};

    // Initialize packet with default values
    packet.lift = 0.0f;
    packet.stretch = 0.0f;
    packet.shift = 0.0f;
    packet.suck_rotate = 0.0f;
    packet.r1 = 0.0f;
    packet.r2 = 0.0f;
    packet.r3 = 0.0f;

    // Map command strings to packet fields
    commandMap["lift"] = &packet.lift;
    commandMap["stretch"] = &packet.stretch;
    commandMap["shift"] = &packet.shift;
    commandMap["suck_rotate"] = &packet.suck_rotate;
    commandMap["r1"] = &packet.r1;
    commandMap["r2"] = &packet.r2;
    commandMap["r3"] = &packet.r3;

    std::string command;
    float value;

    std::cout << "Enter 'exit' to quit the program.\n";

    while (true) {
        // Get command
        std::cout << "\nEnter command (lift/stretch/shift/suck_rotate/r1/r2/r3): ";
        std::cin >> command;

        if (command == "exit") {
            break;
        }

        // Check if command is valid
        if (!commandMap.contains(command)) {
            std::cout << "Invalid command. Please try again.\n";
            clearInputBuffer();
            continue;
        }

        // Get value
        std::cout << "Enter value for " << command << ": ";
        if (!(std::cin >> value)) {
            std::cout << "Invalid value. Please enter a number.\n";
            clearInputBuffer();
            continue;
        }

        // Update value
        *commandMap[command] = value;

        // Display current state
        std::cout << "\nCurrent state:\n";
        std::cout << "  lift: " << packet.lift << "\n";
        std::cout << "  stretch: " << packet.stretch << "\n";
        std::cout << "  shift: " << packet.shift << "\n";
        std::cout << "  suck_rotate: " << packet.suck_rotate << "\n";
        std::cout << "  r1: " << packet.r1 << "\n";
        std::cout << "  r2: " << packet.r2 << "\n";
        std::cout << "  r3: " << packet.r3 << "\n";
        LiftStretch::getInstance().posVelControl(packet.lift, 50, packet.stretch, 50);
        // ShiftSuck::getInstance().posVelControl(packet.shift, 780, packet.suck_rotate, 780);
        Arm::getInstance().posControl(packet.r1, packet.r2, packet.r3);
    }

    std::cout << "Program terminated.\n";
    return 0;
}
